% Generalised Carter and Kohn Smoothing Algorithm to draw from the joint
% distribution of the state vector conditional on the parameters

function [xtt_sample] = Bai_Wang2(F,Q,D,a,Sigtt,xtt,nftot)

    xtt = xtt';

    T = size(xtt,2);
    k = size(F,1);
    
    if isempty(D), D = zeros(k,1); end
    if isempty(a), a = zeros(T,1); end    

    j = 1:nftot; % Number of non-singular elements of Q
    
    xtt_sample = zeros(k,T);

    % Period T

    xtt_sample(:,T)=mvnrnd(xtt(:,T),Sigtt(:,:,T))';
     
    % random numbers
    Z = randn(nftot, T-1)';
    
    % Periods T-1 to 1
    
    mu1 = xtt(1:k-nftot,:);
    mu2 = xtt(k-nftot+1:end,:);
    
    P11 = Sigtt(1:k-nftot,1:k-nftot,:);
    P12 = Sigtt(1:k-nftot,k-nftot+1:end,:);
    P21 = Sigtt(k-nftot+1:end,1:k-nftot,:);
    P22 = Sigtt(k-nftot+1:end,k-nftot+1:end,:);

    F1 = F(1:nftot,1:k-nftot);
    F2 = F(1:nftot,k-nftot+1:end);    
    
    for h=1:(T-1)
        
        at = a(T-h,:)';
        if ~ismatrix(Q)
        Qstart = Q(j,j,T-h);
        else Qstart = Q(j,j);
        end
        
        try
            
        Sigma = P22(:,:,T-h) - P21(:,:,T-h)*inv2(P11(:,:,T-h))*P12(:,:,T-h);
        invSigma = inv(Sigma);
        
        mut = mu2(:,T-h) + P21(:,:,T-h)*inv2(P11(:,:,T-h))*(xtt_sample(nftot+1:end,T-h+1)-mu1(:,T-h));
               
        if nftot == 1
            invSigma = invSigma(1);
        end
        
            m = xtt_sample(1:nftot,T-h+1)- D(1:nftot)*at -F1*xtt_sample(nftot+1:end,T-h+1);
            
            Omega = inv2(invSigma + F2'*inv2(Qstart)*F2);
            mustar = Omega*(invSigma*mut + F2'*inv2(Qstart)*m);     
                   
            draw = mustar + (Z(h,:)*chol(Omega))';        
        
        catch
            
%             disp('ERROR!!!');
            invSigma = schur_pos_inv(Sigtt(:,:,T-h),k-nftot);
            
            mut = mu2(:,T-h) + P21(:,:,T-h)*inv2(P11(:,:,T-h))*(xtt_sample(nftot+1:end,T-h+1)-mu1(:,T-h));

            if nftot == 1
                invSigma = invSigma(1);
            end

            m = xtt_sample(1:nftot,T-h+1) - D(1:nftot)*at - F1*xtt_sample(nftot+1:end,T-h+1);
             
            Omega = inv2(invSigma + F2'*inv2(Qstart)*F2);
            mustar = Omega*(invSigma*mut + F2'*inv2(Qstart)*m);

            draw = mustar + (Z(h,:)*chol(Omega))';
            
        end


        
        xtt_sample(:,T-h) = [xtt_sample(nftot+1:end,T-h+1); draw];
               
    end
    
    xtt_sample = xtt_sample';